package Behaviors;
import Behaviors.NxBehavior;
import Robot.*;
import Utils.Vector2d;
import Utils.Pose2d;

/**
 * 
 *************************************
 ***********************************
 * @author jonathan
 */
public class NxMSGoToPoint extends NxBehavior{
    
    private float MAX_DIST_PROP=4.0f;
    private float MIN_DIST_TO_POINT = 0.1f;
    
    private float exp_param = 3.0f;
    
    public static final int MODE_LINEAR = 0;
    public static final int MODE_EXPONENTIAL = 1;
    
    private int atractorMode;
    
    
    
    //float maxVel= 0.8f;
    private boolean perceptionSchema=false;
    
    public Position2d myPosition;
    NxBehavior embedded;
    public Vector2d goalPoint;

    public NxMSGoToPoint(String name,Position2d myPosition, Vector2d goalPoint, int atractorMode) {
        super(name);
        this.myPosition=myPosition;
        this.goalPoint=goalPoint;
        this.atractorMode = atractorMode;
    }
    
    public NxMSGoToPoint(String name,NxBehavior position,Position2d myPosition, int atractorMode) {
        super(name);
        this.myPosition=myPosition;
        embedded=(NxBehavior)position;
        perceptionSchema=true;
        this.atractorMode = atractorMode;
    }
    
    public Vector2d Value() {
        
        if(perceptionSchema)
        {
            goalPoint = (Vector2d)embedded.Value();
        }

        //get position of robot
        Pose2d pose = myPosition.read(RealPosition2d.READ_MODE_NO_WAIT);
        Vector2d myPos=new Vector2d(pose.getPx(),pose.getPy(),0);
        
        //print position of robot
        //System.out.println("MSGOTOPOINT: Robot position vector:");
        
        //myPos.print(myPos.MODE_XY);
        
        //Vector in direction to point
        Vector2d atractor=goalPoint.sub(myPos);         
        
        //print attractor vector
        //System.out.println("MSGOTOPOINT: Attractor vector:");
        //atractor.print(atractor.MODE_MA);
        
        //Now define the magnitude based on the chosen mode 
        switch(atractorMode){
            
            case MODE_LINEAR:
                atractor = convertLinearMode(atractor);
                break;
            case MODE_EXPONENTIAL:
                atractor = convertExponentialMode(atractor);
                break;
        
        
        }
        //if(atractor.getMag()<10.0f)
            //atractor.setMag(0.0f);
        if(enabled)
            return atractor;
        else
            return new Vector2d();

    }

    //define la variable MAX_DIST_PROP
    public void setMaxDist(float maxDist)
    {
        this.MAX_DIST_PROP = maxDist;
    }
    //define la variable MIN_DIST_TO_POINT
    public void setMinDist(float minDist)
    {
        this.MIN_DIST_TO_POINT = minDist;
    }
    //define las dos variables anteriores
    public void setParamsDist(float minDist, float maxDist)
    {
        this.MAX_DIST_PROP = maxDist;
        this.MIN_DIST_TO_POINT = minDist;
    }



    private Vector2d convertExponentialMode(Vector2d atractor){
        
        float x = (float)atractor.getMag();
        
        //if object is in the closest area, then shutdown atractor
        if(x<=MIN_DIST_TO_POINT)
            return new Vector2d(0,0,Vector2d.MODE_XY);
        else{
            float newMag =(float)( 1 - Math.exp(-exp_param*x));

            atractor.setMag(newMag);
        }
        //atractor.scale(maxVel);
        
        return atractor;
        
        
        
    
    }
    
    
    private Vector2d convertLinearMode(Vector2d atractor){
        
        //If vector greater than influence area, then normalize, else scale and 
        //minimize according to distance to goalPoint
        
            if(atractor.getMag()>MAX_DIST_PROP){
                atractor=atractor.normalize();
                //return atractor.scale(maxVel);
                return atractor;
                
            }
            else{
                if(atractor.getMag()<=MIN_DIST_TO_POINT){
                    
                                 
                    return new Vector2d(0,0,Vector2d.MODE_XY);
                }                    
                else
                    //return atractor.scale(1/MAX_DIST_PROP*maxVel);
                    return atractor.scale(1/MAX_DIST_PROP);
            }
        
    
    }
    //distancia a la cual el vector de velocidad debe disminuir al 37% (0.37).
    public void setExponentialDistance(float x){
        
        exp_param = (float)-Math.log(1 - Math.sqrt(2)/2.0f) / x;
        
        System.out.println(exp_param);
    
    }

    public void changeGoalPoint(Vector2d goalPoint)
    {
        this.goalPoint = goalPoint;
    }


}
